// tf头文件中
#include <tf/transform_listener.h>

struct state
{
    float x;
    float y;
    float yaw;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "get_location");
    ros::NodeHandle n, turtle_node;
    tf::TransformListener listener;
    //.cpp文件中主函数中的主循环
    state robot_pose;
    tf::StampedTransform transform;
    while (ros::ok())
    {
        try
        {
            listener.waitForTransform("map", "laser", ros::Time(0), ros::Duration(1.0));
            listener.lookupTransform("map", "laser", ros::Time(0), transform);
        }
        catch (tf::TransformException &ex)
        {
            ROS_ERROR("%s", ex.what());
            ros::Duration(1.0).sleep();
            break;
        }

        double roll, pitch, yaw;
        tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw, pitch, roll);

        robot_pose.x = transform.getOrigin().x();
        robot_pose.y = transform.getOrigin().y();
        robot_pose.yaw = yaw;

        printf("robot pose: (%.2f,%.2f)  yaw: %.2f", robot_pose.x, robot_pose.y, robot_pose.yaw);
        printf("\n");
        ros::Duration(1.0).sleep();
    }
    return 0;
}
